#include "IPPNS_1034/actionClient.h"
#include <tf2/convert.h>
#include <tf2/utils.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>


using namespace std;


namespace IPPNS{
  

	void pahtserver::printflist(){
		cout<<"数组长度"<<this->Waypoints_list.size()<<endl;
		for (const auto& element : this->Waypoints_list) {
			cout << element << " ";
		}
	}

	/*路径点向输入双向队列输入的接口，保留点击这个点的时候可以为其更改数值的操作接口:
	param:1.任意增添路径点
		2.删改当前路径点
		3.任意插入路径点
		4.一次性删除所有路径点
		5.查看当前的路径点
		6.更改当前路径点的信息
	*/

	/*1.任意增添路径点*/
	void pahtserver::waypoint_add(geometry_msgs::PoseStamped*newpoint){
		//插入新的路径点
		//todo 没有连续输入的接口，后续优化，现为每次单次输入
		if (newpoint!=NULL){
		this->Waypoints_list.push_back(*newpoint);
		}//如果新的路径点已被输入不是空指针
	}

	/*2.任意删改路径点*/
	void pahtserver::waypoint_delete(){
		//easy to operate,通过遍历到目标点再进行删除
		auto nums=current_point;//不改变地质
		auto it = this->Waypoints_list.begin();
		//避免当前点出现越界问题
		while (nums-- && it !=this->Waypoints_list.end()){
			it++;
		}
		this->Waypoints_list.erase(it);
	}


	/*3.任意插入路径点，pos为第几个位置前插入*/
	void pahtserver::waypoint_insert(geometry_msgs::PoseStamped *newpoint,int pos){
		//easy to operate,通过遍历到目标点再进行向前插入
		auto it = this->Waypoints_list.begin();
		//避免当前点出现越界问题
		while (pos<Waypoints_list.size() && pos--){
			it++;
		}
		this->Waypoints_list.insert(it,*newpoint);//插入一个新的路径点
	}

	/*4.一次性删除所有路径点*/
	void pahtserver::waypoint_clear(){
		static string clear_flag;
		ROS_WARN("Ready to clear all list:");
		cout<<"[y or n]"<<endl; 
		cin>>clear_flag;
		if (clear_flag=="y"){
			this->Waypoints_list.clear();
			ROS_WARN("Successfully clear all list");
		}
		
	}

	/*5.查看当前的路径点,并返回当前路径点的信息*/
	geometry_msgs::PoseStamped pahtserver::waypoint_check(){
		auto nums=current_point;//进行一次形参拷贝,以避免更改实参
		auto it = this->Waypoints_list.begin();
		//避免当前点出现越界问题
		while (nums-- && it !=this->Waypoints_list.end()){
			it++;
		}
		cout<<"The data in the "<<current_point<<" place are :"<<endl;
		cout<<"waypoint:"<<*it;
		// cout<<"quaternion:"<<it->quaternion<<endl;
		return *it;//留出返回值，以便后续操作
	}

	/*6.更改当前路径点的信息*/
	void pahtserver::waypoint_data_change(geometry_msgs::PoseStamped *newpoint){
		auto nums=current_point;
		auto it = this->Waypoints_list.begin();
		
		while (nums-- && it !=this->Waypoints_list.end()){
			it++;
		}
		//通过删除当前值，重新添加以改变当前值
		this->Waypoints_list.insert(this->Waypoints_list.erase(it),*newpoint);
	}


	/*初始化自动导航*/
	void naviserver::auto_navigation_init(int mode){
		static string clear_flag;
		//如果取点多于一个则是多点导航，否则是单点导航
		cout<<"[y or n]"<<endl; 
		cin>>clear_flag;
		if (clear_flag=="y"){
			switch (mode)
			{
			case sinpoint_navi_mode:
				ROS_INFO("Sinpoint_navi_mode Started");
				this->auto_single_move();
				break;
			case multipoint_navi_mode:
				ROS_INFO("Multipoint_navi_mode Started");
				this->auto_multi_move();
				break;
			case multipoint_cruise_mode:
				ROS_INFO("Multipoint_cruise_mode Started");
				this->auto_cruise_move();
				break;
			default:
				ROS_ERROR("Non-existen any navi mode !!!!!");
				break;
			}
		}
	}

	//设重启API接口
	bool naviserver::respawn(){

	}
  	//设置紧急停止API接口
	bool naviserver::emergency(){
		return 0;
	} 


	/*用于单点导航，因此直接索取最后一个目标点*/
	void naviserver::auto_single_move(){
		//为target赋予属性
		
		MoveBaseClient ac("move_base",true);
		ROS_INFO("waiting movebase server.");
		if(!ac.waitForServer(ros::Duration(30)))//等待操作服务器可用
		{
			ROS_INFO("not connect movebase server.");
			return;
		}
		ROS_INFO("start to navigation.");
		//直接为pose赋予上最后一个最后一个点信息
		posestamped=this->Waypoints_list.back();
		cout<<"waypoint:"<<posestamped;
		goal.target_pose.header.frame_id = posestamped.header.frame_id;
		goal.target_pose.header.stamp = posestamped.header.stamp;
		goal.target_pose.pose = posestamped.pose;
		//将目标位姿发送到MoveBaseAction服务器
		ac.sendGoal(goal);
		if(this->emergency() || ac.waitForResult(ros::Duration(1000)))//没有及时到达，放弃目标；
		{
			ac.cancelGoal();
			ROS_INFO("time out for arrival goal.");
			while (!respawn_flag)
			{
				cout<<"[0 or 1]"<<endl;
				cin>>respawn_flag;
				/* 在此保留死循环,respawn作为恢复接口 */
			}this->respawn();	
		}
		else
		{
			if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
			{
				ROS_INFO("succeess to arrival goal!");
			}
			else if(this->emergency()){
				ROS_WARN("Emergency stop!!!.");
			}
			else
			{
				ROS_INFO("other reason to failed to arrival goal.");
			}
		}
	}

	/*用于多点导航*/
	void naviserver::auto_multi_move(){
		//为target赋予属性
		MoveBaseClient ac("move_base",true);
		ROS_INFO("waiting movebase server.");
		if(!ac.waitForServer(ros::Duration(30)))//等待操作服务器可用
		{
			ROS_INFO("not connect movebase server.");
			return;
		}
		ROS_INFO("start to navigation.");
		for (const auto& element : this->Waypoints_list){
			cout<<"waypoint:"<<element;
			goal.target_pose.header.frame_id = element.header.frame_id;
			goal.target_pose.header.stamp = element.header.stamp;
			goal.target_pose.pose = element.pose;
			ac.sendGoal(goal);
			//10s没有及时到达，放弃目标
			if(this->emergency() || !ac.waitForResult(ros::Duration(10)))
			{
				ac.cancelGoal();
				ROS_INFO("time out for arrival goal.");
				while (!respawn_flag)
				{
					cout<<"[0 or 1]"<<endl;
					cin>>respawn_flag;
					/* 在此保留死循环,respawn作为恢复接口 */
				}this->respawn();
			}
			else
			{
				if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
				{
					ROS_INFO("succeess to arrival goal!");
					/*!!!!有问题，应该是最后一个点*/
					// while (!respawn_flag)
					// {
					// 	cout<<"[0 or 1]"<<endl;
					// 	cin>>respawn_flag;
					// 	/* 在此保留死循环,respawn作为恢复接口,确定是否在这个点暂停 */
					// }this->respawn();
				}
				else if(this->emergency()){
					ROS_WARN("Emergency stop!!!.");
				}
				else
				{
					ROS_INFO("other reason to failed to arrival goal.");
				}
			}
		}
	}


	/*用于多点巡航，利用三次B样条曲线计算每发布路径*/
	// void naviserver::auto_cruise_move(){
	// 	pub=node.advertise<nav_msgs::Path>("move_base/GlobalPlanner/plan",10);
	// 	nav_msgs::Path path;
		
	// 	geometry_msgs::PoseStamped new_goal;
	// 	new_goal.header.frame_id="map";new_goal.header.stamp=ros::Time::now();
	// 	path.header.frame_id="map";path.header.stamp=ros::Time::now();

	// 	typedef Eigen::Vector2d Point2f;
	// 	vector<Point2f> discretePoints;
		
	// 	Point2f current_point,last_point;

	// 	for (const auto& element : this->Waypoints_list){
			
	// 		Eigen::Vector2d coord_xyz(element.pose.position.x,element.pose.position.y);
	// 		discretePoints.push_back(coord_xyz);
	// 	}
	// 	float stride=0.4;
	// 	//先载入第一个点
	// 	path.poses.push_back(this->Waypoints_list.front());

	// 	for (int i = 0; i < discretePoints.size()-1; i++) {
	// 		Point2f xy[4];
	// 		xy[0] = (discretePoints[i] + 4 * discretePoints[(i + 1) % discretePoints.size()] + discretePoints[(i + 2) % discretePoints.size()]) / 6;
	// 		xy[1] = -(discretePoints[i] - discretePoints[(i + 2) % discretePoints.size()]) / 2;
	// 		xy[2] = (discretePoints[i] - 2 * discretePoints[(i + 1) % discretePoints.size()] + discretePoints[(i + 2) % discretePoints.size()]) / 2;
	// 		xy[3] = -(discretePoints[i] - 3 * discretePoints[(i + 1) % discretePoints.size()] + 3 * discretePoints[(i + 2) % discretePoints.size()] - discretePoints[(i + 3) % discretePoints.size()]) / 6;
	// 		for (double t = 0; t <= 1; t += stride) {
	// 			Point2f current_point = Point2f(0, 0);
	// 			for (int j = 0; j < 4; j++) {
	// 				current_point += xy[j] * pow(t, j);
	// 			}
	// 			 if(i!=0){
	// 				double yaw= atan2((current_point[1]-last_point[1]), (current_point[0]-last_point[0]));
	// 				// std::cout<<"yaw"<<yaw<<std::endl;
	// 				tf2::Quaternion qtn;
	// 				new_goal.pose.position.x=last_point[0];
    //       			new_goal.pose.position.y=last_point[1];
	// 				qtn.setRPY(0,0,yaw);//转换为四元数
	// 				new_goal.pose.orientation.x = qtn.getX();
	// 				new_goal.pose.orientation.y = qtn.getY();
	// 				new_goal.pose.orientation.z = qtn.getZ();
	// 				new_goal.pose.orientation.w = qtn.getW();
	// 				path.poses.push_back(new_goal);
	// 			}
	// 			last_point=current_point;
	// 			cout<<"totalPoints:"<<current_point<<endl;
	// 		}
	// 	}
	// 	ros::Rate rate(10);
	// 	while (ros::ok())
	// 	{
	// 		pub.publish(path);
	// 		rate.sleep();

	// 	}
		
	// }
	void naviserver::auto_cruise_move(){
		pub=node.advertise<nav_msgs::Path>("move_base/GlobalPlanner/plan",10);
		nav_msgs::Path path;
		
		geometry_msgs::PoseStamped new_goal;
		new_goal.header.frame_id="map";new_goal.header.stamp=ros::Time::now();
		path.header.frame_id="map";path.header.stamp=ros::Time::now();

		typedef Eigen::Vector2d Point2f;
		vector<Point2f> discretePoints;
		
		Point2f current_point,last_point;

		for (const auto& element : this->Waypoints_list){
			
			Eigen::Vector2d coord_xyz(element.pose.position.x,element.pose.position.y);
			discretePoints.push_back(coord_xyz);
		}
		float stride=0.4;
		//先载入第一个点
		path.poses.push_back(this->Waypoints_list.front());



		for (int i = 0; i < discretePoints.size()-1; i++) {
			Point2f xy[4];
			xy[0] = (discretePoints[i] + 4 * discretePoints[(i + 1) % discretePoints.size()] + discretePoints[(i + 2) % discretePoints.size()]) / 6;
			xy[1] = -(discretePoints[i] - discretePoints[(i + 2) % discretePoints.size()]) / 2;
			xy[2] = (discretePoints[i] - 2 * discretePoints[(i + 1) % discretePoints.size()] + discretePoints[(i + 2) % discretePoints.size()]) / 2;
			xy[3] = -(discretePoints[i] - 3 * discretePoints[(i + 1) % discretePoints.size()] + 3 * discretePoints[(i + 2) % discretePoints.size()] - discretePoints[(i + 3) % discretePoints.size()]) / 6;
			for (double t = 0; t <= 1; t += stride) {
				Point2f current_point = Point2f(0, 0);
				for (int j = 0; j < 4; j++) {
					current_point += xy[j] * pow(t, j);
				}
				 if(i!=0){
					double yaw= atan2((current_point[1]-last_point[1]), (current_point[0]-last_point[0]));
					// std::cout<<"yaw"<<yaw<<std::endl;
					tf2::Quaternion qtn;
					new_goal.pose.position.x=last_point[0];
          			new_goal.pose.position.y=last_point[1];
					qtn.setRPY(0,0,yaw);//转换为四元数
					new_goal.pose.orientation.x = qtn.getX();
					new_goal.pose.orientation.y = qtn.getY();
					new_goal.pose.orientation.z = qtn.getZ();
					new_goal.pose.orientation.w = qtn.getW();
					path.poses.push_back(new_goal);
				}
				last_point=current_point;
				cout<<"totalPoints:"<<current_point<<endl;
			}
		}
		ros::Rate rate(10);
		while (ros::ok())
		{
			pub.publish(path);
			rate.sleep();

		}
		
	}

}